#include "matrixmath.h"

using namespace Math;

#define PARALLELTHRESHOLD 0.001

Vector3 Math::Vector4to3(Vector4 v){
    return Vector3(v.x, v.y, v.z);
}

void Math::PrintVector(Vector3 v){
    std::cout<<"Vector3: ["<<v.x<<", "<<v.y<<", "<<v.z<<"]\n";
}
void Math::PrintVector(Vector4 v){
    std::cout<<"Vectorr: ["<<v.x<<", "<<v.y<<", "<<v.z<<", "<<v.w<<"]\n";
}

void Math::PrintMatrix3x3(Matrix3x3 m){
    std::cout<<"Matrix3x3:\n["<<m.x1<<", "<<m.y1<<", "<<m.z1<<"]\n["<<m.x2<<", "<<m.y2<<", "<<m.z2<<"]\n["<<m.x3<<", "<<m.y3<<", "<<m.z3<<"]\n";
}

Vector3 Math::i(){
    return Vector3(1, 0, 0);
}

Vector3 Math::j(){
    return Vector3(0, 1, 0);
}

Vector3 Math::k(){
    return Vector3(0, 0, 1);
}

Matrix3x3 Math::Identity3x3(){
    Matrix3x3 I;
    I.x1 = I.y2 = I.z3 = 1;
    return I;
}

Matrix3x3 Math::MatrixAddition3x3(Matrix3x3 a, Matrix3x3 b){
    Matrix3x3 c;
    c.x1 = a.x1+b.x1;
    c.x2 = a.x2+b.x2;
    c.x3 = a.x3+b.x3;
    c.y1 = a.y1+b.y1;
    c.y2 = a.y2+b.y2;
    c.y3 = a.y3+b.y3;
    c.z1 = a.z1+b.z1;
    c.z2 = a.z2+b.z2;
    c.z3 = a.z3+b.z3;
    return c;
}

Matrix3x3 Math::MatrixScalarMultiplication3x3(Matrix3x3 m, float s){
    m.x1*=s;
    m.x2*=s;
    m.x3*=s;
    m.y1*=s;
    m.y2*=s;
    m.y3*=s;
    m.z1*=s;
    m.z2*=s;
    m.z3*=s;
    return m;
}

Matrix3x3 Math::MatrixMultiplication3x3(Matrix3x3 a, Matrix3x3 b){
    Matrix3x3 c;

    c.x1 = a.x1*b.x1 + a.y1*b.x2 + a.z1*b.x3;
    c.x2 = a.x2*b.x1 + a.y2*b.x2 + a.z2*b.x3;
    c.x3 = a.x3*b.x1 + a.y3*b.x2 + a.z3*b.x3;

    c.y1 = a.x1*b.y1 + a.y1*b.y2 + a.z1*b.y3;
    c.y2 = a.x2*b.y1 + a.y2*b.y2 + a.z2*b.y3;
    c.y3 = a.x3*b.y1 + a.y3*b.y2 + a.z3*b.y3;

    c.z1 = a.x1*b.z1 + a.y1*b.z2 + a.z1*b.z3;
    c.z2 = a.x2*b.z1 + a.y2*b.z2 + a.z2*b.z3;
    c.z3 = a.x3*b.z1 + a.y3*b.z2 + a.z3*b.z3;

    return c;
}

Vector3 Math::MatrixVectorMultiplication3f(Matrix3x3 m, Vector3 v){
    Vector3 r;

    r.x = m.x1*v.x + m.y1*v.y + m.z1*v.z;
    r.x = m.x2*v.x + m.y2*v.y + m.z2*v.z;
    r.x = m.x3*v.x + m.y3*v.y + m.z3*v.z;

    return r;
}

Vector4 Math::MatrixVectorMultiplication4f(Matrix4x4 m, Vector4 v){
    Vector4 r;

    r.x = m.x1*v.x + m.y1*v.y + m.z1*v.z + m.w1*v.w;
    r.y = m.x2*v.x + m.y2*v.y + m.z2*v.z + m.w2*v.w;
    r.z = m.x3*v.x + m.y3*v.y + m.z3*v.z + m.w3*v.w;
    r.w = m.x4*v.x + m.y4*v.y + m.z4*v.z + m.w4*v.w;

    return r;
}

Matrix3x3 Math::SelfTensorProduct3f(Vector3 v){
    Matrix3x3 m;
    m.x1 = v.x*v.x;
    m.x2 = v.x*v.y;
    m.x3 = v.x*v.z;
    m.y1 = v.x*v.y;
    m.y2 = v.y*v.y;
    m.y3 = v.y*v.z;
    m.z1 = v.x*v.z;
    m.z2 = v.y*v.z;
    m.z3 = v.z*v.z;
    return m;
}

Vector3 Math::ScalarProduct(Vector3 v, float s){
    v.x*=s;
    v.y*=s;
    v.z*=s;
    return v;
}

Vector3 Math::VectorAddition3f(Vector3 a, Vector3 b){
    Vector3 c;
    c.x = a.x+b.x;
    c.y = a.y+b.y;
    c.z = a.z+b.z;
    return c;
}

float Math::Magnitude(Vector3 v){
    return sqrt(v.x*v.x + v.y*v.y + v.z*v.z);
}

Vector3 Math::Normalise(Vector3 v){
    return ScalarProduct( v, 1.0/Magnitude(v));
}

float Math::DotProduct(Vector3 a, Vector3 b){
    return (a.x*b.x) + (a.y*b.y) + (a.z*b.z);
}

float Math::AngleBetween(Vector3 a, Vector3 b){
    return acos( DotProduct(a, b)/(Magnitude(a)*Magnitude(b)) );
}

bool Math::ParallelVector(Vector3 a, Vector3 b){
    return AngleBetween(a, b)<PARALLELTHRESHOLD ? true : false;
}

bool Math::OpposedVector(Vector3 a, Vector3 b){
    return AngleBetween(a, ScalarProduct(b, -1))<PARALLELTHRESHOLD ? true : false;
}

Vector3 Math::CrossProduct(Vector3 a, Vector3 b){
    Vector3 c;

    if(ParallelVector(a, b)){
       return c;
    }

    c.x = a.y*b.z - a.z*b.y;
    c.y = a.z*b.x - a.x*b.z;
    c.z = a.x*b.y - a.y*b.x;
    return c;
}

Matrix3x3 Math::VectorCrossProductMatrix3f(Vector3 v){
    Matrix3x3 m;
    m.x2 = v.z;
    m.x3 = -v.y;
    m.y1 = -v.z;
    m.y3 = v.x;
    m.z1 = v.y;
    m.z2 = -v.x;
    return m;
}

Vector3 Math::RotateVector(Vector3 v, Vector3 axis, float angle){
    Matrix4x4 rot;
    Vector4 quat(v, angle);
    float c = cos(angle);
    float s = sin(angle);
    float t = 1-cos(angle);
    float x = axis.x;
    float y = axis.y;
    float z = axis.z;

    rot.x1 = t*x*x + c;
    rot.x2 = t*x*y - s*z;
    rot.x3 = t*x*z + s*y;
    rot.x4 = 0;
    rot.y1 = t*x*y + s*z;
    rot.y2 = t*y*y + c;
    rot.y3 = t*y*z - s*x;
    rot.y4 = 0;
    rot.z1 = t*x*z - s*y;
    rot.z2 = t*y*z + s*x;
    rot.z3 = t*z*z + c;
    rot.z4 = 0;
    rot.w1 = 0;
    rot.w2 = 0;
    rot.w3 = 0;
    rot.w4 = 1;

    v = Vector4to3(MatrixVectorMultiplication4f(rot, quat));

    v = Normalise(v);

    return v;
}
